
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       slew_limiter.c
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "slew_limiter.h"

#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void slew_limiter_ctor(slew_limiter_t sl, const float *slew_rate_max, const float *slew_rate_tau)
{
    sl->slew_rate_max = slew_rate_max;
    sl->slew_rate_tau = slew_rate_tau;

    lpf_set_cutoff1(&sl->slew_filter, SLEW_LIMITER_DERIVATIVE_CUTOFF_FREQ);
    lpf_reset(&sl->slew_filter, 0.0f);
}

/*
  apply filter to sample, returning multiplier between 0 and 1 to keep
  output within slew rate
 */
float slew_limiter_modifier(slew_limiter_t sl, float sample, float dt)
{
    if (!math_flt_positive(dt)) {
        return 1.0f;
    }
    
    if (*(sl->slew_rate_max) <= 0) {
        sl->_output_slew_rate = 0.0f;
        return 1.0f;
    }

    // Calculate a low pass filtered slew rate
    const float slew_rate = lpf_apply2(&sl->slew_filter, (sample - sl->last_sample) / dt, dt);
    sl->last_sample = sample;

    uint32_t now_ms = time_millis();
    const float decay_alpha = fminf(dt, *(sl->slew_rate_tau)) / (*(sl->slew_rate_tau));

    // Store a series of positive slew rate exceedance events
    if (!sl->_pos_event_stored && slew_rate > (*(sl->slew_rate_max))) {
        if (sl->_pos_event_index >= SLEW_LIMITER_N_EVENTS) {
            sl->_pos_event_index = 0;
        }
        sl->_pos_event_ms[sl->_pos_event_index] = now_ms;
        sl->_pos_event_index++;
        sl->_pos_event_stored = true;
        sl->_neg_event_stored = false;
    }

    // Store a series of negative slew rate exceedance events
    if (!sl->_neg_event_stored && slew_rate < - (*(sl->slew_rate_max))) {
        if (sl->_neg_event_index >= SLEW_LIMITER_N_EVENTS) {
            sl->_neg_event_index = 0;
        }
        sl->_neg_event_ms[sl->_neg_event_index] = now_ms;
        sl->_neg_event_index++;
        sl->_neg_event_stored = true;
        sl->_pos_event_stored = false;
    }

    // Find the oldest event time
    uint32_t oldest_ms = now_ms;
    for (uint8_t index = 0; index < SLEW_LIMITER_N_EVENTS; index++) {
        if (sl->_pos_event_ms[index] < oldest_ms) {
            oldest_ms = sl->_pos_event_ms[index];
        }
        if (sl->_neg_event_ms[index] < oldest_ms) {
            oldest_ms = sl->_neg_event_ms[index];
        }
    }

    // Decay the peak positive and negative slew rate if they are outside the window
    // Never drop PID gains below 10% of configured value
    if (slew_rate > sl->_max_pos_slew_rate) {
        sl->_max_pos_slew_rate = fminf(slew_rate, 10.0f * (*(sl->slew_rate_max)));
        sl->_max_pos_slew_event_ms = now_ms;
    } else if (now_ms - sl->_max_pos_slew_event_ms > SLEW_LIMITER_WINDOW_MS) {
        sl->_max_pos_slew_rate *= (1.0f - decay_alpha);
    }

    if (slew_rate < -sl->_max_neg_slew_rate) {
        sl->_max_neg_slew_rate = fminf(-slew_rate, 10.0f * (*(sl->slew_rate_max)));
        sl->_max_neg_slew_event_ms = now_ms;
    } else if (now_ms - sl->_max_neg_slew_event_ms > SLEW_LIMITER_WINDOW_MS) {
        sl->_max_neg_slew_rate *= (1.0f - decay_alpha);
    }

    const float raw_slew_rate = 0.5f*(sl->_max_pos_slew_rate + sl->_max_neg_slew_rate);

    // Apply a further reduction when the oldest exceedance event falls outside the window required for the
    // specified number of exceedance events. This prevents spikes due to control mode changed, etc causing
    // unwanted gain reduction and is only applied to the slew rate used for gain reduction
    float modifier_input = raw_slew_rate;
    if (now_ms - oldest_ms > (SLEW_LIMITER_N_EVENTS + 1) * SLEW_LIMITER_WINDOW_MS) {
        const float oldest_time_from_window = 0.001f*(float)(now_ms - oldest_ms - (SLEW_LIMITER_N_EVENTS + 1) * SLEW_LIMITER_WINDOW_MS);
        modifier_input *= expf(-oldest_time_from_window / (*(sl->slew_rate_tau)));
    }

    // Apply a filter to increases in slew rate only to reduce the effect of gusts and large controller
    // setpoint changes
    const float attack_alpha = fminf(2.0f * decay_alpha, 1.0f);

    sl->_modifier_slew_rate = (1.0f - attack_alpha) * sl->_modifier_slew_rate + attack_alpha * modifier_input;
    sl->_modifier_slew_rate = fminf(sl->_modifier_slew_rate, modifier_input);

    sl->_output_slew_rate = (1.0f - attack_alpha) * sl->_output_slew_rate + attack_alpha * raw_slew_rate;
    sl->_output_slew_rate = fminf(sl->_output_slew_rate, raw_slew_rate);

    // Calculate the gain adjustment
    float mod;
    if (sl->_modifier_slew_rate > (*(sl->slew_rate_max))) {
        mod = (*(sl->slew_rate_max)) / ((*(sl->slew_rate_max)) + SLEW_LIMITER_MODIFIER_GAIN * (sl->_modifier_slew_rate - (*(sl->slew_rate_max))));
    } else {
        mod = 1.0f;
    }

    return mod;
}

/*------------------------------------test------------------------------------*/


